#ifndef OPTIMIZER_H
#define OPTIMIZER_H

#include <vector>
#include <ceres/ceres.h>
#include <unordered_map>
#include "PBABlocks.hpp"

#include "../../Calibration.h"


namespace Estimator
{

class PoseLocalParameterization;
class LandMarkLocalParameterization;
class Managing;
class Frame;
class LandMarkPLX;

class Optimizer
{
  public:
    Optimizer(Calibration calibration): mCalibration(calibration),
                                        mPoseLocalPtr_(new PoseLocalParameterization),
                                        mpLandMarkLocalPtr_(new LandMarkLocalParameterization)
    {
      SetOptions();
    }
    virtual ~Optimizer() = default;
    Optimizer(const Optimizer&) = delete;

    void SetOptions();

    void InitializeByManager(const Managing* pManager);

    void AddPoseBlock(const Frame* frame);

    void AddLandMarkBlock(const LandMarkPLX* landPLX);

    void GenerateCostFunctionBeta(const Managing* pManaging);

    void SolveOptimization();

    void SetOrdering();
 
    size_t NumberofPoses() const {return mHashFrames.size();}

    std::vector<std::array<double,12> > WritePoses() const {
      
      std::vector<std::pair<size_t/*landmark id*/, std::array<double,12>> > vPoses;
      std::vector<std::array<double,12> > result_Poses;
      for(const auto tem_pair : mHashFrames)
      {
        Eigen::Matrix<double,3,4,Eigen::RowMajor> pose;
        pose = tem_pair.second.getEstimate();
        std::array<double,12> pose_data;
        memcpy(pose_data.data(), pose.data(), sizeof(double) * 12);

        vPoses.push_back(std::make_pair(tem_pair.first,pose_data));
      }

      std::sort(vPoses.begin(),vPoses.end(),[](std::pair<size_t/*landmark id*/, std::array<double,12>> a,
                                              std::pair<size_t/*landmark id*/, std::array<double,12>> b){
                                                return a.first < b.first;
                                              });

      std::transform(vPoses.begin(), vPoses.end(), std::back_inserter(result_Poses), [](std::pair<size_t/*landmark id*/, std::array<double,12>> tem){
        return tem.second;
      });

      // std::array<double,12> res = result_Poses.at(3);
      std::cout << vPoses[0].first << std::endl;
      std::cout <<result_Poses.size() << std::endl;
      return result_Poses;
    }

    

  public:
    const Calibration mCalibration;

    // std::vector<PoseBlock> mvPoseBlock; 
    // std::vector<LandMarkBlock> mvLandMarkBlock;

    std::unordered_map<size_t/*landmark id*/, PoseBlock> mHashFrames;
    std::unordered_map<size_t/*frame id */, LandMarkBlock> mHashLandMarks;

    PoseLocalParameterization* mPoseLocalPtr_;    // shouldn't be deleted in deconstructor, cause ceres already do that for u
    LandMarkLocalParameterization* mpLandMarkLocalPtr_;

    ceres::Problem mOptimizationProblem;
    ceres::Solver::Options mOptions_;

};

class OptimizerTest : public Optimizer
{
  void WriteResult();

  void WriteResultintoFile(std::string resultfile);

  // size_t nEdge_ = 0;
  std::vector<std::pair<size_t, std::array<double, 12>>> mvPoseGroundTruth;
  std::vector<std::pair<size_t, std::array<double, 5>>> mvLandMarkGroundTruth; // [theta, 3d normalized main anchor]
};
}

#endif // OPTIMIZER_H